#include "pch.h"
#include "SimulationModel.h"
#include "ShapeMatchingConstraint.h"
#include "DistanceConstraint.h"
#include "VolumeConstraint.h"

CSimulationModel::~CSimulationModel()
{
	__clear();
}

bool CSimulationModel::pumpConstraints(std::vector<CConstraint*>& voConstraints, ConstraintType vType) const
{
	std::vector<CConstraint*> Constraints;
	switch (vType)
	{
	case DISTANCE:
		for (auto Constraint : m_DistanceConstraints)
			Constraints.push_back(Constraint);
		break;
	case VOLUME:
		for (auto Constraint : m_VolumeConstraints)
			Constraints.push_back(Constraint);
		break;
	case SHAPE_MATCHING:
		for (auto Constraint : m_ShapeMatchingConstraints)
			Constraints.push_back(Constraint);
		break;
	default:
		break;
	}

	if (Constraints.size() > 0)
	{
		voConstraints = Constraints;
		return true;
	}

	return false;
}

bool CSimulationModel::pumpConstraintGroups(std::vector<std::vector<int>>& voConstraintGroups, ConstraintType vType) const
{
	switch (vType)
	{
	case DISTANCE:
		voConstraintGroups = m_DistanceConstraintGroups;
		break;
	case VOLUME:
		voConstraintGroups = m_VolumeConstraintGroups;
		break;
	case SHAPE_MATCHING:
		voConstraintGroups = m_ShapeMatchingConstraintGroups;
		break;
	default:
		break;
	}

	return voConstraintGroups.size() > 0 ? true : false;
}

void CSimulationModel::addSimulationParticle(const Eigen::Vector3d& vPos, const Eigen::Vector3d& vGravity)
{
	m_particles.addParticle(vPos);
	m_particles.getAcceleration(m_particles.getSize() - 1) = vGravity;
}

void CSimulationModel::generateConstraints()
{
	__initConstraintGroups(m_DistanceConstraints, m_DistanceConstraintGroups);
	__initConstraintGroups(m_VolumeConstraints, m_VolumeConstraintGroups);
	__initConstraintGroups(m_ShapeMatchingConstraints, m_ShapeMatchingConstraintGroups);
	m_RotationMatrices.resize(m_ShapeMatchingConstraints.size());
}

bool CSimulationModel::addShapeMatchingConstraint(int vNumberOfParticles, const std::vector<double>& vStiffness, const std::vector<int>& vParticleIndices, const std::vector<int>& vNumClusters)
{
	CShapeMatchingConstraint* pConstraint = new CShapeMatchingConstraint(vNumberOfParticles);
	bool res = pConstraint->initializeConstraint(m_particles, vParticleIndices, vNumClusters, vStiffness);
	if (res)
		m_ShapeMatchingConstraints.push_back(pConstraint);

	return res;
}


bool CSimulationModel::addDistanceConstraint(int vParticle1, int vParticle2, double vStiffness)
{
	CDistanceConstraint *pConstraint = new CDistanceConstraint();
	const bool res = pConstraint->initializeConstraint(m_particles, std::pair<int, int>(vParticle1, vParticle2), vStiffness);
	if (res)
		m_DistanceConstraints.push_back(pConstraint);

	return res;
}

bool CSimulationModel::addVolumeConstraint(const std::vector<unsigned int>& vParticles, double vStiffness)
{
	CVolumeConstraint *pConstraint = new CVolumeConstraint();
	const bool res = pConstraint->initializeConstraint(m_particles, vParticles, vStiffness);
	if (res)
		m_VolumeConstraints.push_back(pConstraint);

	return res;
}

void CSimulationModel::setRotationMatrices(int vIndex, const Eigen::Matrix3d& vRotationMatrix)
{
	_ASSERTE(vIndex >= 0 && vIndex < m_RotationMatrices.size());

	m_RotationMatrices[vIndex] = vRotationMatrix;
}

void CSimulationModel::__clear()
{
	for (auto Constraint : m_DistanceConstraints)
		delete Constraint;
	for (auto Constraint : m_VolumeConstraints)
		delete Constraint;
	for (auto Constraint : m_ShapeMatchingConstraints)
		delete Constraint;
}